/*
 * OdometryByModule.cpp
 *
 *  Created on: 22 янв. 2013
 *      Author: alex
 */

#include "includes.h"
#include "OdometryByModule.h"

#include <cstdio>

OdometryByModule::OdometryByModule()
{
	printf("odometry_goto_init\nodometry_track_init\n");
	odometry_track_init();
	odometry_goto_init();
	odo_initialize();

}

OdometryByModule::~OdometryByModule()
{

}


Point OdometryByModule::goTo(const Point& dest)
{
		odometry_goto_set_goal(&og, dest.x, dest.y);
		//if (algorithm.configuration.verbosity > 0)
		{
			printf("$GOTO_POSITION,%f,%f\n", og.state.goal_x, og.state.goal_y);
		}

		// Move until we have reached the target position
		while (og.result.atgoal == 0) {
			// Update position and calculate new speeds
			odometry_track_step(og.track);
			odometry_goto_step(&og);

			// Set speed
			khepera3_drive_set_speed(og.result.speed_left, og.result.speed_right);

			// Print the current position
			//if (algorithm.configuration.verbosity > 1)
			{
				//printf("$POSITION,%f,%f,%f\n", og.track->result.x, og.track->result.y, og.track->result.theta);
			}

		}

		// Announce when we have reached the target position
		//if (algorithm.configuration.verbosity > 0)
		{
			//printf("$POSITION,%f,%f,%f\n", og.track->result.x, og.track->result.y, og.track->result.theta);
			//fflush(stdout);
		}

		return Point( og.track->result.x,og.track->result.y,og.track->result.theta);


}

Point OdometryByModule::turnTo(double goal_theta)
{
	double diff_theta;

	// Announce the new target heading
	//if (verbosity > 0) {
		printf("$GOTO_HEADING,%lf\n", goal_theta);
	//}

	// Move until we have reached the target position
	while (1) {
		// Update position and calculate new speeds
		odometry_track_step(og.track);

		// Calculate the current heading error
		diff_theta = goal_theta - og.track->result.theta;
		while (diff_theta > PI) {
			diff_theta -= 2 * PI;
		}
		while (diff_theta < -PI) {
			diff_theta += 2 * PI;
		}

		// Termination condition
		if (fabs(diff_theta) < 0.01) {
			break;
		}

		// Set speed
		khepera3_drive_set_speed_differential_bounded(og.configuration.speed_max, 0, 0, diff_theta * 8., 1);

		// Print the current position
		//if (verbosity > 1) {
			//printf("$POSITION,%f,%f,%f\n", og.track->result.x, og.track->result.y, og.track->result.theta);
		//}
	}

	// Stop the motors
	khepera3_drive_set_speed(0, 0);

	// Announce when we have reached the target heading
	//if (verbosity > 0) {
		printf("$POSITION,%f,%f,%f\n", og.track->result.x, og.track->result.y, og.track->result.theta);
		fflush(stdout);
	//}
		return Point( og.track->result.x,og.track->result.y,og.track->result.theta);
}


void OdometryByModule::odo_initialize()
{
	// Read command line arguments
	//verbosity = commandline_option_value_int("-v", "--verbosity", 1);

	//if (algorithm.configuration.verbosity > 0)
			printf("odometry track start\nodometry_goto_start\n");

	// Start a new track
	odometry_track_start(&ot);
	odometry_goto_start(&og, &ot);
	//char c=getchar();
	//scanf("%c",c);
	// Put the wheels in normal (control) mode
	khepera3_drive_start();
	//if (algorithm.configuration.verbosity > 0)
		printf("odometry init\n");

}


